
//#include <iostream>
#include "kalman.h"
#include <stdio.h>


int main()
{

    char da_c[] = {0xe0,0x00, 0x00, 0x00, 0x06, 0xc0};
    char db_c[] = {0xe0,0x01, 0x00, 0x00, 0x0d, 0x89};
    char dc_c[] = {0xe0,0x02, 0x00, 0x00, 0x0e, 0x32};

    int data_a[6] = {0};
    int data_b[6] = {0};
    int data_c[6] = {0};
    double x=0,y=0;

    DataTransform(da_c, data_a, 6);
    DataTransform(db_c, data_b, 6);
    DataTransform(dc_c, data_c, 6);

    position(data_a, data_b, data_c, x, y);
    printf("%f, %f\n", x, y);

    return 0;
}
